#include "charger.h"


charger_t charger = {
    .state = CHARGE_INIT ,
    .battery_L = {
        .state = BAT_IDLE,
        .pre_state = BAT_IDLE ,
        .adc_voltage = {
            .data_arr = {0},
            .data = 0,
            .avg_data = 0
        },
        .adc_current = {
            .data_arr = {0},
            .data = 0,
            .avg_data = 0
        },
        .adc_vout = {
            .data_arr = {0},
            .data = 0,
            .avg_data = 0
        },

        .current_gain = Current_gain_L,

        .kfp_current = {
            20, //估算协方差. 初始化值为 0.02
            0, //卡尔曼增益. 初始化值为 0
            1, //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏. 初始化值为 0.001
            10000, //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好. 初始化值为 1
            100 //卡尔曼滤波器输出. 初始化值为 0
        },

        .err.Byte = 0 ,

        .short_prot_value = L_BAT_Current_To_ADC(5),
        .over_current_value = 750 ,
        .current_stage = CURRENT_05A ,

        .pid = {
            .Kp = KP ,
            .Ki = KI ,
            .Kd = KD ,
            .err1 = 0 ,
            .err2 = 0 ,
            .out_min = OUT_min ,
            .out_max = OUT_max ,
        },
        .pwm_ccr = 0,

        .volPid = {
            .Kp = KP_Vout ,
            .Ki = KI_Vout ,
            .Kd = KD_Vout ,
            .err1 = 0 ,
            .err2 = 0 ,
            .out_min = OUT_min_Vout ,
            .out_max = OUT_max_Vout ,
        },

        .volCtlCCR = 0 ,

        .dis_charge = dis_charge_L,
        .en_charge = en_charge_L,
        .regula_pwm = regula_pwm_L,
        .lenth_detect = lenth_det_L,
        .pwm_off = pwm_off_L,
        .pwm_on = pwm_on_L,
        .key = {
            .short_press_time = SHORT_PRESS_TIME ,
            .long_press_time_max = LONG_PRESS_TIME_MAX ,
            .long_press_time_min = LONG_PRESS_TIME_MIN ,
            .press_time = 0,
            .short_flag = 0,
            .short_wait = 0,
            .get_key_value = get_key_L
        } ,
    } ,


    .battery_R = {
        .state = BAT_IDLE,
        .pre_state = BAT_IDLE ,
        .adc_voltage = {
            .data_arr = {0},
            .data = 0,
            .avg_data = 0
        },
        .adc_current = {
            .data_arr = {0},
            .data = 0,
            .avg_data = 0
        },
        .adc_vout = {
            .data_arr = {0},
            .data = 0,
            .avg_data = 0
        },

        .current_gain = Current_gain_R,

        .kfp_current = {
            20, //估算协方差. 初始化值为 0.02
            0, //卡尔曼增益. 初始化值为 0
            1, //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏. 初始化值为 0.001
            10000, //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好. 初始化值为 1
            100 //卡尔曼滤波器输出. 初始化值为 0
        },

        .err.Byte = 0 ,

        .short_prot_value = R_BAT_Current_To_ADC(5),
        .over_current_value = 750 ,
        .current_stage = CURRENT_05A ,

        .pid = {
            .Kp = KP ,
            .Ki = KI ,
            .Kd = KD ,
            .err1 = 0 ,
            .err2 = 0 ,
            .out_min = OUT_min ,
            .out_max = OUT_max ,
        },
        .pwm_ccr = 0,

        .volPid = {
            .Kp = KP_Vout ,
            .Ki = KI_Vout ,
            .Kd = KD_Vout ,
            .err1 = 0 ,
            .err2 = 0 ,
            .out_min = OUT_min_Vout ,
            .out_max = OUT_max_Vout ,
        },

        .volCtlCCR = 0 ,

        .dis_charge = dis_charge_R,
        .en_charge = en_charge_R,
        .regula_pwm = regula_pwm_R,
        .lenth_detect = lenth_det_R,
        .pwm_off = pwm_off_R,
        .pwm_on = pwm_on_R,
        .key = {
            .short_press_time = SHORT_PRESS_TIME ,
            .long_press_time_max = LONG_PRESS_TIME_MAX ,
            .long_press_time_min = LONG_PRESS_TIME_MIN ,
            .press_time = 0,
            .short_flag = 0,
            .short_wait = 0,
            .get_key_value = get_key_R
        } ,
    } ,

    .Vin = {
        .data_arr = {0},
        .data = 0,
        .avg_data = 0
    },
    .vin_voltage = 0,
    
    .error_OT   = 0 ,
    .error_LT   = 0 ,
    .lt_cnt     = 0 ,
    .lt_rec_cnt = 0 ,
    .ot_cnt     = 0 ,
    .ot_rec_cnt = 0 ,
    .temp = { 0 } ,

    .ot_value     = Over_Temp_ADC     ,
    .ot_rec_value = Rec_Over_Temp_ADC ,
    .lt_value     = Low_Temp_ADC      ,
    .lt_rec_value = Rec_Low_Temp_ADC  ,
    .kfp_C_posit = {
        20, //估算协方差. 初始化值为 0.02
        0, //卡尔曼增益. 初始化值为 0
        1, //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏. 初始化值为 0.001
        10000, //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好. 初始化值为 1
        100 //卡尔曼滤波器输出. 初始化值为 0
    },


    .temp_pid = {
        .Kp = KP_Temp ,
        .Ki = KI_Temp ,
        .Kd = KI_Temp ,
        .err1 = 0 ,
        .err2 = 0 ,
        .out_max = OUT_max_Temp ,
        .out_min = OUT_min_Temp
    },

    .adj_max_value = MAX_TEMP_ADC_VALUE,
    .adj_min_value = MIN_TEMP_ADC_VALUE
};


/**
 * @description: 获取电压/电流真实值 
 * @param {uint16_t} value
 * @param {actual_value_t} type
 * @return {uint16_t vol or current 单位:mv / mA}
 */
uint32_t get_actual_value(uint16_t value , actual_value_t type , Battery_t * pBat){
    uint32_t ret = 0;
    switch (type)
    {
        case C_Vin_voltage:     /*C+电压*/
            ret = value * VDD_volt / Charge_Vol_Gain / ADC_MAX * 1000;
            break;

        case Vout_voltage:      /*充电电压*/
            ret = value * VDD_volt / Vout_Gain / ADC_MAX * 1000;
            break;

        case Battery_voltage:   /*电池电压*/
            ret = value * VDD_volt / BAT_VOLTAGE_Gain / ADC_MAX * 1000;
            break;

        case charge_Current:    /*充电电流*/
            ret = value * VDD_volt / Current_Sample_Res / pBat->current_gain / ADC_MAX * 1000;
            break;

        default:
            break;
    }
    return ret;
}


/**
 * @description: 闪充检测
 * @param {charger_t *} pCharger
 * @return {*}
 */
void flash_charger_detect(charger_t * pCharger){
    static uint16_t cnt = 0 , cnt_clear = 0;
    #define FLASH_CHARGER_VOLTAGE_MIN   5500  /*闪充电压最小值*/
    #define FLASH_CHARGER_VOLTAGE       6500  /*闪充电压最大值*/
    #define FLASH_CHARGER_TIME          500   /*闪充检测时间*/

    if(pCharger->vin_voltage > FLASH_CHARGER_VOLTAGE){
        if(++cnt > FLASH_CHARGER_TIME){
            cnt = 0;
            pCharger->flash_charger = 1;
        }
    }
    else if(pCharger->vin_voltage < FLASH_CHARGER_VOLTAGE_MIN){
        if(++cnt_clear > FLASH_CHARGER_TIME){
            cnt_clear = 0;
            pCharger->flash_charger = 0;
        }
    }
    else{
        cnt = 0;
        cnt_clear = 0;
    }

    /*给定最大占空比*/
    if(pCharger->state == CHARGE_INIT)    pCharger->Epwm_max_duty = 20;
    else if (pCharger->state == CHARGE_NOMOL){
        pCharger->Epwm_max_duty = 4200  * 100 / pCharger->vin_voltage ;
    }
}



/**
 * @description: 充电器温度保护 低温/过温
 * @param {charger_t *} charge
 * @return {*}
 */
void Temp_protect(charger_t * charge){
    /*过温保护*/
    if(charge->error_OT == 0){
        if(charge->temp.avg_data <= charge->ot_value){
            if(charge->ot_cnt < Temp_Fault_time){
                charge->ot_cnt++;
            } else{
                charge->ot_cnt = 0;
                charge->error_OT = 1;
            }
        } else{
            if(charge->ot_cnt) 
                charge->ot_cnt = 0;
        }
    }
    /*过温恢复*/
    #if(Temp_recovery_EN)
    else{
        if(charge->temp.avg_data >= charge->ot_rec_value){
            if(charge->ot_rec_cnt < Temp_Fault_Recov_time){
                charge->ot_rec_cnt++;
            }
            else{
                charge->ot_rec_cnt = 0;
                charge->error_OT = 0;
                charge->battery_L.temp_pro_recover = 1;
                charge->battery_R.temp_pro_recover = 1;
            }
        }
        else{
            if(charge->ot_rec_cnt)
                charge->ot_rec_cnt = 0;
        }
    }
    #endif


    /*低温保护*/
    if(charge->error_LT == 0){
        if(charge->temp.avg_data >= charge->lt_value){
            if(charge->lt_cnt < Temp_Fault_time){
                charge->lt_cnt++;
            }
            else{
                charge->lt_cnt = 0;
                charge->error_LT = 1;
            }
        }
        else{
            if(charge->lt_cnt)
                charge->lt_cnt = 0;
        }
    }
    #if(Temp_recovery_EN)
    else{
        if(charge->temp.avg_data <= charge->lt_rec_value){
            if(++charge->lt_rec_cnt >= Temp_Fault_Recov_time){
                charge->lt_rec_cnt = 0;
                charge->error_LT = 0;
                charge->battery_L.temp_pro_recover = 1;
                charge->battery_R.temp_pro_recover = 1;
            }
        }
        else{
            if(charge->lt_rec_cnt)
                charge->lt_rec_cnt = 0;
        }
    }
    #endif

}


void Open_voltage_detect(void){
    if(++charger.open_sample_time < OpenSample_Con_time){
        charger.vol_samp_flag = 0;
    }
    else if(charger.open_sample_time < OpenSample_period){
        charger.vol_samp_flag = 1;
    }
    else {
        charger.open_sample_time = 0;
    }
}


/**
 * @description: 电池充电开启/关闭
 * @param {Battery_t *} pBat
 * @return {*}
 */
void Battery_charge_en(Battery_t * pBat){
    if(pBat->state < BAT_Charge_off && pBat->state > BAT_IDLE)
    {
        pBat->vol_samp_flag = charger.vol_samp_flag;
        if(pBat->current_display_state == NORMAL_CHARGE){
            if(pBat->vol_samp_flag){
                pBat->pid_delay = 0;
                pBat->pid_start = 0;
                pBat->pwm_off();
                
                if(++pBat->dis_chg_delay > 10){
                    pBat->dis_chg_delay = 0;
                    pBat->dis_charge();
                }
            }
            else{
                pBat->dis_chg_delay = 0;
                pBat->en_charge();
                if(++pBat->pid_delay > 10){
                    pBat->pid_delay = 0;
                    pBat->pid_start = 1;
                    pBat->pwm_on();
                }
            }
        }
        else{
            pBat->pwm_off();
            pBat->pid_start = 0;
            pBat->pid_delay = 0;
            pBat->dis_charge();
        }
    }
    else{
        pBat->vol_samp_flag = 0;
    }
}


/**
 * @description: 获取输入电压
 * @param {charger_t *} charge
 * @return {*}
 */
void Get_vin_voltage(charger_t * pCharger){
    /*滤波*/
    // pCharger->Vin.avg_data = Filter_calculate(pCharger->Vin.data_arr , pCharger->Vin.data , DATA_NUM);
    pCharger->Vin.avg_data = KalmanFilter(&pCharger->kfp_C_posit , pCharger->Vin.data);

    /*获取C+实际电压*/
    pCharger->vin_voltage = get_actual_value(pCharger->Vin.avg_data , C_Vin_voltage , NULL);

    pCharger->temp.avg_data = Filter_calculate(pCharger->temp.data_arr , pCharger->temp.data , DATA_NUM);
}


/**
 * @description: 电池进程
 * @param {Battery_t *} pBat
 * @return {*}
 */
void Battery_process(Battery_t * pBat){
    /*AD数据处理*/
    if(pBat->state < BAT_Charge_off && pBat->state > BAT_IDLE){
        if(pBat->vol_samp_flag == 1){
            pBat->adc_voltage.avg_data = Filter_calculate(pBat->adc_voltage.data_arr , pBat->adc_voltage.data , DATA_NUM);
            pBat->voltage = get_actual_value(pBat->adc_voltage.avg_data , Battery_voltage, NULL); /*电池电压*/

            /*滤波*/
            pBat->adc_vout.avg_data = Filter_calculate(pBat->adc_vout.data_arr , pBat->adc_vout.data , DATA_NUM);
            pBat->vout_voltage = get_actual_value(pBat->adc_vout.avg_data , Vout_voltage, NULL);      /*充电输出电压*/
        }
        else{
            pBat->adc_current.avg_data = KalmanFilter(&pBat->kfp_current , pBat->adc_current.data);
            pBat->current = get_actual_value(pBat->adc_current.avg_data , charge_Current, pBat);      /*充电电流*/
            
        }
    }
    else{
        pBat->adc_voltage.avg_data = Filter_calculate(pBat->adc_voltage.data_arr , pBat->adc_voltage.data , DATA_NUM);
        pBat->voltage = get_actual_value(pBat->adc_voltage.avg_data , Battery_voltage , NULL); /*电池电压*/
        
        // pBat->adc_vout.avg_data = Filter_calculate(pBat->adc_vout.data_arr , pBat->adc_vout.data , DATA_NUM);
        
        pBat->adc_current.avg_data = KalmanFilter(&pBat->kfp_current , pBat->adc_current.data);

        pBat->current = get_actual_value(pBat->adc_current.avg_data , charge_Current , pBat);      /*充电电流*/

        // pBat->vout_voltage = get_actual_value(pBat->adc_vout.avg_data , Vout_voltage , NULL);      /*充电输出电压*/
    }

    /*采集实时电压*/
    pBat->realtime_voltage = get_actual_value(pBat->adc_voltage.data , Battery_voltage, NULL);/*实时电压*/

    /*电量判断*/
    if(pBat->state > BAT_IDLE){
        Bat_Satge_judg(pBat);
    }
    else{
        pBat->vol_stage.stage = BAT_STAGE_ZERO;
    }
    
    
    /*电池插入检测*/
    Battery_insert_detect(pBat);

    if(pBat->state == BAT_PreChage || pBat->state == BAT_WAIT_ACTIVAT){
        pBat->current_set = 50;
    }
    else if(pBat->state == BAT_Charge){
        /*按键检测*/
        Key_detect(&pBat->key);
        /*充电状态下调节充电电流*/
        current_set_process(pBat);
        /*设置充电电流*/    
        pBat->current_set = (uint8_t)pBat->current_stage * 500 + 500;
        
        /*过流检测*/
        Over_current_detect(pBat);
    }

    if(pBat->state != BAT_Charge){
        pBat->current_display_state = NORMAL_CHARGE;
    }

    /*故障检测*/
    if(pBat->state > BAT_IDLE && pBat->state < BAT_Charge_off){
        Vout_over_detect(pBat);
    }    
}


uint16_t Get_chargeDuty(uint16_t voltage , uint16_t target , Battery_t *pBat){
    if(voltage > target){
        return 0;
    }

    uint16_t duty = 0 ;
    uint16_t diffvol = target - voltage;

    if(diffvol > 130 && diffvol <= 200){
        duty = EPWM_DUTY(charger.Epwm_max_duty) * 70 / 100;
    }
    else if(diffvol > 90 && diffvol <= 130){
        duty = EPWM_DUTY(charger.Epwm_max_duty) * 60 / 100;
    }
    else if(diffvol > 50 && diffvol <= 90){
        duty = EPWM_DUTY(charger.Epwm_max_duty) * 50 / 100;
    }
    else if(diffvol > 0 && diffvol <= 50){
        duty = EPWM_DUTY(charger.Epwm_max_duty) * 40 / 100;
    }
    
    return duty;
}

void Trickle_state(Battery_t * pBat){
    if(pBat->state == BAT_Charge){
        if(pBat->voltage > 4050){
            pBat->constVolCancelCnt = 0;
            if(pBat->charge_mode == ConstCurrent){
                pBat->modeChagCnt++;
                if(pBat->modeChagCnt > 200){
                    pBat->modeChagCnt = 0;
                    pBat->charge_mode = ConstVoltage;
                }
            }
        }
        else if(pBat->voltage < 4000){
            pBat->modeChagCnt = 0;
            if(pBat->charge_mode == ConstVoltage){
                pBat->constVolCancelCnt++;
                if(pBat->constVolCancelCnt > 1000){
                    pBat->constVolCancelCnt = 0;
                    pBat->charge_mode = ConstCurrent;
                }
            }
        }

        if(pBat->charge_mode == ConstCurrent){
            pBat->volCtlCCR = EPWM_DUTY(charger.Epwm_max_duty);
        }
        else if(pBat->charge_mode == ConstVoltage){
            pBat->volCtlCCR = Get_chargeDuty(pBat->voltage , 4210 , pBat);
        }
    }
    else{
        pBat->constVolCancelCnt = 0;
        pBat->modeChagCnt = 0;
        pBat->charge_mode = ConstCurrent;
        pBat->volCtlCCR = EPWM_DUTY(charger.Epwm_max_duty);
    }
}



/**
 * @description: pid调节电流过程
 * @param {Battery_t*} pBat
 * @return {*}
 */
void Pid_calculat_process(Battery_t* pBat){
    /*pid调节电流*/
    if(pBat->state > BAT_IDLE && pBat->state < BAT_Charge_off){
        /*电压环*/
        Trickle_state(pBat);
        if(pBat->pid_start)
        {
            /*电流环*/
            if(pBat->current_display_state == NORMAL_CHARGE){
                pBat->pwm_on();
                if(charger.reduce_current == 1){
                    if(pBat->current_set == 2000){
                        pBat->pwm_ccr += Pid_calculate(&pBat->pid , 1700 , pBat->current);
                    }
                    else{
                        pBat->pwm_ccr += Pid_calculate(&pBat->pid , pBat->current_set , pBat->current);
                    }   
                }
                else{
                    pBat->pwm_ccr += Pid_calculate(&pBat->pid , pBat->current_set , pBat->current);
                }
                /*限幅*/
                pBat->pwm_ccr = LIMIT(pBat->pwm_ccr , EPWM_DUTY(10) , EPWM_DUTY(charger.Epwm_max_duty));
                pBat->pwm_ccr = MIN(pBat->pwm_ccr , pBat->volCtlCCR);   /*限制电流*/

                /*调节占空比*/
                pBat->regula_pwm(pBat->pwm_ccr);
            }
            else{
                pBat->pwm_off();
                pBat->pwm_ccr = 0;
                pBat->pid.err1 = 0;
                pBat->pid.err2 = 0;
            }
        }
        else{
            pBat->pwm_off();
        }
    }
    else{
        pBat->pwm_ccr = 0;
        /*清除误差*/
        pBat->pid.err1 = 0;
        pBat->pid.err2 = 0;
        pBat->pwm_off();

        pBat->volCtlCCR = 0;
        pBat->volPid.err1 = 0;
        pBat->volPid.err2 = 0;

        pBat->constVolCancelCnt = 0;
        pBat->modeChagCnt = 0;
        pBat->charge_mode = ConstCurrent;
    }
}



extern uint8_t gQC_protocol_end;
/**
 * @description: 
 * @param {charger_t *} pCharge
 * @return {*}
 */
void Charger_state_machine(charger_t * pCharge){
    switch(pCharge->state){
        case CHARGE_INIT:
            if(gQC_protocol_end){
                gQC_protocol_end = 0;
                pCharge->state = CHARGE_NOMOL;
            }

            break;

        case CHARGE_NOMOL:

            break;

        case CHARGE_PULL_OUT:

            break;

        default:
            break;
    }
}




void Temp_control(charger_t * pChg){
    pChg->temp_pwm_duty += Temp_Pid_calculate(&pChg->temp_pid , pChg->adj_max_value , pChg->temp.avg_data);

    pChg->temp_pwm_duty = LIMIT(pChg->temp_pwm_duty , 10 , pChg->Epwm_max_duty);
}



void Limt_max_temp(charger_t * pChg){
    static uint8_t delay_cnt = 0 , cancel_cnt = 0;
    if(pChg->reduce_current == 0){
        cancel_cnt = 0;
        if(pChg->temp.avg_data < pChg->adj_max_value){
            if(++delay_cnt > 100){
                delay_cnt = 0;
                pChg->reduce_current = 1;
            }
        }
        else{
            if(delay_cnt) delay_cnt--;
            else{
                delay_cnt = 0;
            }
        }
    }
    else if(pChg->reduce_current == 1){
        delay_cnt = 0;
        if(pChg->temp.avg_data > pChg->adj_min_value){
            if(++cancel_cnt > 100){
                cancel_cnt = 0;
                pChg->reduce_current = 0;
            }
        }
        else{
            if(cancel_cnt) cancel_cnt--;
            else{
                cancel_cnt = 0;
            }
        }
    }
}








